﻿using System;
using System.ComponentModel;
using System.Collections.Generic;
using System.IO;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;
using Accord.Math;
using Accord.Math.Decompositions;
using xn;

namespace ActionRecognition
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        Kinect kinect;
        WriteableBitmap img = new WriteableBitmap(640, 480, 96, 96, PixelFormats.Rgb24, null);
        public readonly AsyncOperation asyncOperation;
        List<LineJoint> listLineJoint;
        StreamWriter writer;

        public MainWindow()
        {
            InitializeComponent();
            Closing += new CancelEventHandler(MainWindow_Closing);
            asyncOperation = AsyncOperationManager.CreateOperation(new object());

        }

        void MainWindow_Closing(object sender, CancelEventArgs e)
        {
            if (kinect != null) kinect.keepUpdate = false;
            if (writer != null)
            {
                writer.Write(']');
                writer.Close();
            }
        }

        private void buttonConnect_Click(object sender, RoutedEventArgs e)
        {
            try
            {
                kinect = new Kinect("openni.xml");
                kinect.userUpdateEventHandler += new Kinect.UserUpdateEventHandler(kinect_userUpdateEventHandler); 
                kinect.StateChangedEventHandler += new EventHandler(kinect_StateChangedEventHandler);
                writer = new StreamWriter(String.Format("../../data/{0}.txt", DateTime.Now.ToString("HH.mm.dd-MM-yy")));
                writer.Write('[');
                listLineJoint = new List<LineJoint>();
                listLineJoint.Add(new LineJoint(0, 1));     //head-neck
                listLineJoint.Add(new LineJoint(1, 3));     //neck-right shoulder
                listLineJoint.Add(new LineJoint(3, 4));     //right shoulder - right elbow
                listLineJoint.Add(new LineJoint(4, 5));     //right elbow - right hand
                listLineJoint.Add(new LineJoint(1, 6));     //neck - left shoulder
                listLineJoint.Add(new LineJoint(6, 7));     //left shoulder - left elbow
                listLineJoint.Add(new LineJoint(7, 8));     //left elbow - left hand
                listLineJoint.Add(new LineJoint(3, 2));     //right shoulder - torso
                listLineJoint.Add(new LineJoint(6, 2));     //left shoulder - torso
                listLineJoint.Add(new LineJoint(2, 9));     //torso - right hip
                listLineJoint.Add(new LineJoint(9, 10));    //right hip - right knee
                listLineJoint.Add(new LineJoint(10, 11));   //right knee - right foot
                listLineJoint.Add(new LineJoint(2, 12));    //torso - left hip
                listLineJoint.Add(new LineJoint(12, 13));   //left hip - left knee
                listLineJoint.Add(new LineJoint(13, 14));   //left knee- left foot

                buttonConnect.IsEnabled = false;
                buttonDisconnect.IsEnabled = true;
            }
            catch (Exception ex)
            {
                MessageBox.Show("Koneksi tidak berhasil dilakukan! Alasan : " + ex.Message);
            }
        }

        private void buttonDisconnect_Click(object sender, RoutedEventArgs e)
        {
            if (kinect != null) kinect.keepUpdate = false;
            mainImage.Source = null;

            buttonConnect.IsEnabled = true;
            buttonDisconnect.IsEnabled = false;
        }

        void kinect_StateChangedEventHandler(object sender, EventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                switch (kinect.state)
                {
                    case State.NewUser: labelInfo.Content = "User ditemukan buatlah pose Phi"; break;
                    case State.Uncalibrated: labelInfo.Content = "Kalibrasi gagal, buatlah kembali pose Phi"; break;
                    case State.PoseDetected: labelInfo.Content = "Pose phi ditemukan"; break;
                    case State.Calibrating: labelInfo.Content = "Kalibrasi dimulai"; break;
                    case State.Tracking: labelInfo.Content = "Tracking stick figure user"; break;
                    case State.LostUser: labelInfo.Content = "User hilang"; break;
                }
            });
        }


        void kinect_userUpdateEventHandler(object sender, UserUpdateEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                mainCanvas.Children.Clear();
                foreach (User user in kinect.userList)
                {
                    double[,] m = new double[9, 3];
                    for (int i = 0; i < user.listSkel.Count; i++)
                    {
                        Point3D p = user.listSkel[i].transformation.position.position;
                        float conf = user.listSkel[i].transformation.position.fConfidence;
                        if (conf > 0)
                        {
                            //Console.WriteLine("User {0} {1} is : ({2},{3},{4}), Confidence : {5}", user.id, user.listSkeletonInfo[i].jointName, p.X, p.Y, p.Z, conf);
                            Point3D q = kinect.depthGenerator.ConvertRealWorldToProjective(p);
                            user.listSkel[i].p = new Point(q.X, q.Y);
                            DrawCircle(q.X, q.Y, Colors.Blue);
                        }

                        //m[i, 0] = p.X;
                        //m[i, 1] = p.Y;
                        //m[i, 2] = p.Z;
                        if (i < 9) {
                            Point3D t = user.listSkel[2].transformation.position.position;
                            m[i, 0] = p.X - t.X;
                            m[i, 1] = p.Y - t.Y;
                            m[i, 2] = p.Z - t.Z;
                        }
                    }

                    SingularValueDecomposition svd = new SingularValueDecomposition(m, false, false);
                    for (int i = 0; i < svd.Diagonal.Length-1; i++) writer.Write(svd.Diagonal[i] + ",");
                    writer.Write(svd.Diagonal[svd.Diagonal.Length-2] + ";"); 
                    writer.WriteLine();
                    
                    //DRAW LINE
                    foreach (LineJoint join in listLineJoint)
                    {
                        if (user.listSkel[join.j1].transformation.position.fConfidence > 0 &&
                            user.listSkel[join.j2].transformation.position.fConfidence > 0)
                        Drawline(user.listSkel[join.j1].p, user.listSkel[join.j2].p, Colors.DarkGreen);
                    }
                }
            });

            asyncOperation.SynchronizationContext.Send(
                delegate
                {
                    labelFrame.Content = "Frame #" + e.frame;
                    img.Lock();
                    img.WritePixels(new Int32Rect(0, 0, 640, 480), kinect.imageMetaData.ImageMapPtr, (int)kinect.imageMetaData.DataSize, img.BackBufferStride);
                    img.Unlock();
                    mainImage.Source = img;
                },
                null
            );
        }

        void DrawCircle(float positionX, float positionY, Color color)
        {
            Ellipse ellipse = new Ellipse
            {
                Fill = new SolidColorBrush(color),
                Width = 20,
                Height = 20,
                Margin = new Thickness(positionX-10, positionY-10, 0, 0)
            };

            mainCanvas.Children.Add(ellipse);
        }

        void Drawline(Point p1, Point p2, Color color)
        {
            Line line = new Line()
            {
                X1 = p1.X,
                Y1 = p1.Y,
                X2 = p2.X,
                Y2 = p2.Y,
                Stroke = new SolidColorBrush(color),
                StrokeThickness = 5
            };
            mainCanvas.Children.Add(line);
        }
    }

    public class LineJoint
    {
        public int j1;
        public int j2;
        public LineJoint(int _j1, int _j2)
        {
            j1 = _j1;
            j2 = _j2;
        }
    }
}
